#include "../../include/arm/assembler.h"
#include "../../include/arm/v7m.h"
#include "../../include/asm/ptrace.h"

ENTRY(PendSV_Handler)
    cpsid	i

    mrs	r0, psp

    tst lr, #0x10
    it eq
    vstmdbeq r0!, {d8 - d15} /* if lr_bit4 == 0 then save */
    addne r0, r0, #64

    stmfd r0!, {lr}

    bl __pendsv_do_work

    ldmfd r0!, {lr}

    tst lr, #0x10
    it eq
    vldmiaeq r0!, {d8 - d15}
    subne r0, r0, #64

    msr psp, r0

    cpsie i
    bx lr
ENDPROC(PendSV_Handler)
